#!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
from std_msgs.msg import String
from geometry_msgs.msg import Twist
from geometry_msgs.msg import PoseStamped

# 1st CB
def GoalReceiveCallback(msg):
	tag=1
	rospy.loginfo("tag:%d" %tag)	
	rospy.Subscriber("scan", LaserScan, RushCallback) # 2nd CB
	rospy.spin()

# 2nd CB
def RushCallback(data):
	flag = 1
	for x in range(th_a, th_b):
		#rospy.loginfo("angular:%d,range:%0.2f" %(x,data.ranges[x]))
		if data.ranges[x] < dis:
			rospy.loginfo("range:%0.2f, angular %d" %(data.ranges[x], x))
			flag = 0
			break
	rospy.loginfo("flag:%d" %(flag))
	if (flag):
		for i in range(0,100):
			pub.publish(vel)
		rospy.loginfo("rush! %d" %(vel.linear.x))

	
if __name__ == '__main__':
	# values
	tag = 0
	vel = Twist()
	dis = 2.0 	#min distance
	rangee = 30 	#angular range
	th_a = -rangee/2
	th_b = rangee/2
	vel.linear.x = 1590.0 	#set velocity
	vel.angular.z = 90.0 	#default

	# init ROS
	rospy.init_node('LastRush', anonymous=False)
	pub = rospy.Publisher("/car/cmd_vel", Twist, queue_size=10)
	rospy.Subscriber("/move_base_simple/goal", PoseStamped, GoalReceiveCallback) # 1st CB
	rospy.spin()

